/*倒立摆控制与仿真*/
#include "pendulum.hpp"
#include "controller.hpp"

int main()
{
    Simulator sim1;
    PID_Controller *mpidinner = new PID_Controller(&sim1, 10, 0, 100, 100);
    PID_Controller *mpidouter = new PID_Controller(&sim1, 0.64056276, 2.6843531, 0.80895114, 0.75105566);
    InversePendulum *mpendulum = new InversePendulum(&sim1);
    FUConstant(minu, &sim1);
    FUSum(msuminner, &sim1);
    FUSum(msumouter, &sim1);
    FUOutput(mouttheta, &sim1);
    FUOutput(moutx, &sim1);
    minu->Set_OutValue(0);
    sim1.connectU(minu, msumouter);
    sim1.connectU(msumouter, mpidouter, 0);
    sim1.connectU(mpidouter, 0, msuminner);
    sim1.connectU(msuminner, mpidinner, 0);
    sim1.connectU(mpidinner, 0, mpendulum, 0);
    sim1.connectU(mpendulum, 0, msumouter);
    sim1.connectU(mpendulum, 2, msuminner);
    msumouter->Set_InputGain(-1);
    msuminner->Set_InputGain(-1);
    sim1.connectU(mpendulum, 0, moutx);
    sim1.connectU(mpendulum, 2, mouttheta);
    sim1.Initialize();
    // sim1.Set_SimStep(0.01);
    sim1.Simulate(20);
    sim1.Plot();
    return 0;
}
